﻿using MySoftLab.DapFlashCore.Probes.DapAccess.Interfaces;
using MySoftLab.DapFlashCore.Utils;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Net.NetworkInformation;
using System.Text;
using System.Threading.Tasks;
using static System.Collections.Specialized.BitVector32;
using static System.Runtime.InteropServices.JavaScript.JSType;

namespace MySoftLab.DapFlashCore.Probes.DapAccess
{
    /// <summary>
    /// SWO statuses.
    /// </summary>
    public enum SWOStatus
    {
        DISABLED = 1,
        CONFIGURED = 2,
        RUNNING = 3,
        ERROR = 4,
    }

    public class DAP_LED
    {
        public const byte DAP_DEBUGGER_CONNECTED = 0;
        public const byte DAP_TARGET_RUNNING = 1;
    }

    public class DelayResult<T>
    {
        public bool IsValue { get; set; }

        public T? Value { get; set; }

        public Func<T?>? Action { get; set; }

        public DelayResult(T value)
        {
            IsValue = true;
            Value = value;
        }

        public DelayResult(Func<T?> action)
        {
            IsValue = false;
            Action = action;
        }
    }


    public class DapAccessCmsisDap : DAPAccessIntf
    {
        // CMSIS-DAP values
        public const byte AP_ACC = 1 << 0;
        public const byte DP_ACC = 0 << 0;
        public const byte READ = 1 << 1;
        public const byte WRITE = 0 << 1;
        public const byte VALUE_MATCH = 1 << 4;
        public const byte MATCH_MASK = 1 << 5;

        // # APnDP constants.
        public const uint DP = 0;
        public const uint AP = 1;

        // Bitmasks for AP register address fields.
        public const uint A32 = 0x0000000c;

        private readonly DapInterface _interface;
        private string? _uniqueId;
        private string _vendorName;
        private string _productName;
        private (uint, uint) _vidPid;
        private bool _deferredTransfer = true;
        private CMSISDAPProtocol _protocol;
        private int? _packetCount = null;
        private int _frequency = 1000000; // 1MHz default clock
        private PORT? _dapPort = null;
        private LinkedList<Transfer> _transferList = new LinkedList<Transfer>();
        private Command _crntCmd;
        private int? _packetSize = null;
        private LinkedList<Command> _commandsToRead = new LinkedList<Command>();
        private byte[] _commandResponseBuf = new byte[0];
        private (int, int, int) _cmsisDapVersion = CMSISDAPVersion.V1_0_0;
        private string _fwVersion = "";
        private bool _hasOpenedOnce = false;
        private bool _isOpen = false;
        private Dictionary<ID, object?> _cachedInfo = new Dictionary<ID, object?>();
        private object _lock = new object();
        private bool _hasSwoUart = false;
        private SWOStatus _swoStatus = SWOStatus.DISABLED;
        private int _capabilities;
        private int _swoBufferSize = 0;

        // Map from AP/DP and 2-bit register address to the enums used by pydapaccess.
        private Dictionary<(uint, uint), REG> REG_ADDR_TO_ID_MAP = new Dictionary<(uint, uint), REG>
        { //# APnDP A32
            { ( 0,    0x0 ) , REG.DP_0x0 },
            { ( 0,    0x4 ) , REG.DP_0x4 },
            { ( 0,    0x8 ) , REG.DP_0x8 },
            { ( 0,    0xC ) , REG.DP_0xC },
            { ( 1,    0x0 ) , REG.AP_0x0 },
            { ( 1,    0x4 ) , REG.AP_0x4 },
            { ( 1,    0x8 ) , REG.AP_0x8},
            { ( 1,    0xC ) , REG.AP_0xC}
        };

        public void Lock()
        {
            Monitor.Enter(_lock);
        }

        public void Unlock()
        {
            Monitor.Exit(_lock);
        }

        public DapAccessCmsisDap(string? uniqueId, DapInterface @interface)
        {
            _uniqueId = uniqueId;
            _interface = @interface;

            _vendorName = _interface.VendorName;
            _productName = _interface.ProductName;
            _vidPid = (_interface.Vid, _interface.Pid);
            _protocol = new CMSISDAPProtocol(_interface);
            _crntCmd = new Command(0);

        }

        public override string VendorName => _vendorName;

        public override string ProductName => _productName;

        public override (uint, uint) VidPid => _vidPid;

        public static List<DapAccessCmsisDap> GetConnectedDevices()
        {
            List<DapAccessCmsisDap> allDapLinks = new List<DapAccessCmsisDap>();

            var allInterfaces = GetInterfaces();
            foreach (var @interface in allInterfaces)
            {
                try
                {
                    var newDaplink = new DapAccessCmsisDap(null, @interface);
                    allDapLinks.Add(newDaplink);
                }
                catch (Exception ex)
                {
                    Console.WriteLine("Failed to get unique id:" + ex.Message);
                }
            }

            return allDapLinks;
        }

        /// <summary>
        /// Get the connected USB devices
        /// </summary>
        private static DapInterface[] GetInterfaces()
        {
            return HidApiUSB.GetAllConnectedInterfaces();
        }

        public override (int, int, int) ProtocolVersion
        {
            get
            {
                return _cmsisDapVersion;
            }
        }

        public override string FirmwareVersion => _fwVersion;

        /// <summary>
        /// Determine the CMSIS-DAP protocol version.
        /// </summary>
        private void ReadProtocolVersion()
        {
            // The fallback version to use when version parsing fails depends on whether v2 bulk endpoints are used
            // (unfortunately conflating transport with protocol).
            var fallbackProtocolVersion = _interface.IsBulk ? CMSISDAPVersion.V2_0_0 : CMSISDAPVersion.V1_0_0;

            var protocolVersionStr = Identify(ID.CMSIS_DAP_PROTOCOL_VERSION);

            // Just in case we don't get a valid response, default to the lowest version (not including betas).
            if (string.IsNullOrEmpty(protocolVersionStr))
            {
                _cmsisDapVersion = fallbackProtocolVersion;
            }
            // Deal with DAPLink broken version number, where these versions of the firmware reported the DAPLink
            // version number for DAP_INFO_FW_VER instead of the CMSIS-DAP version, due to a misunderstanding
            // based on unclear documentation.
            else if (_vidPid == HidHelper.ARM_DAPLINK_ID && (protocolVersionStr == "0254" || protocolVersionStr == "0255"))
            {
                _cmsisDapVersion = CMSISDAPVersion.V2_0_0;
            }
            else
            {
                // Convert the version to a 3-tuple for easy comparison.
                // 1.2.3 will be converted to (1,2,3), 1.10 to (1,1,0), and so on.
                //
                // There are two version formats returned from the reference CMSIS-DAP code: 2-field and 3-field.
                // The older versions return versions like "1.07" and "1.10", while recent versions return "1.2.0"
                // or "2.0.0".
                //
                // Some CMSIS-DAP compatible debug probes from various vendors return the probe's firmware version
                // rather than protocol version (like DAPLink versions 0254 and 0255 do) due to a misunderstanding
                // based on unclear documentation. These cases are handled by the additional error checking below.
                //
                // Note that the exact version identified here is not that important, as it's not used much in
                // this code (so far at least). There are also DAP_Info Capability bits for availability of certain
                // commands that should be used instead of checking the version.
                try
                {
                    var fwVersion = protocolVersionStr.Split('.');
                    int major = Convert.ToInt32(fwVersion[0]);
                    int minor, patch;
                    // Handle version of the form "1.10" by treating the two digits after the dot as minor and patch.
                    if (fwVersion.Length == 2 && fwVersion[1].Length == 2)
                    {
                        minor = Convert.ToInt32(fwVersion[1][0]);
                        patch = Convert.ToInt32(fwVersion[1][1]);
                    }
                    else
                    {
                        minor = fwVersion.Length > 1 ? Convert.ToInt32(fwVersion[1]) : 0;
                        patch = fwVersion.Length > 2 ? Convert.ToInt32(fwVersion[2]) : 0;
                    }
                    _cmsisDapVersion = (major, minor, patch);
                }
                catch
                {
                    // One of the protocol version fields had a non-numeric character, indicating it is not a valid
                    // CMSIS-DAP version number. Default to the lowest version.
                    _cmsisDapVersion = fallbackProtocolVersion;
                }

                // Catch the beta release versions of CMSIS-DAP, 0.01 and 0.02, and raise them to 1.0.0.
                if (_cmsisDapVersion.Item1 == 0 && _cmsisDapVersion.Item2 == 0)
                {
                    _cmsisDapVersion = CMSISDAPVersion.V1_0_0;
                }
                // Validate the version against known CMSIS-DAP major versions.
                else if (!CMSISDAPVersion.MajorVersions().Contains(_cmsisDapVersion.Item1))
                {
                    _cmsisDapVersion = fallbackProtocolVersion;
                }
            }
        }

        public override bool IsOpen => _isOpen;

        internal LinkedList<Command> CommandsToRead { get => _commandsToRead; }
        internal Command CrntCmd { get => _crntCmd; }

        public override void Open()
        {
            lock (_lock)
            {
                if (_interface == null)
                {
                    throw new DeviceError("Unable to open device with no interface");
                }

                if (_isOpen)
                {
                    return;
                }

                _interface.Open();

                // If this probe has already been opened and examined previously, we don't need to examine it again.
                if (_hasOpenedOnce)
                {
                    InitDeferredBuffers();
                    if (_hasSwoUart)
                    {
                        SwoDisable();
                        _swoStatus = SWOStatus.DISABLED;
                    }
                    _isOpen = true;
                    return;
                }

                // TODO
                if (false)
                {

                }
                else
                {
                    var packetCount = Identify(ID.MAX_PACKET_COUNT);
                    if (packetCount == null)
                    {
                        throw new ArgumentException();
                    }

                    _packetCount = int.Parse(packetCount.ToString());
                }

                // Get the protocol version.
                ReadProtocolVersion();

                // Read the firmware version if the protocol supports it.
                // THe PRODUCT_FW_VERSION ID was added in versions 1.3.0 (HID) and 2.1.0 (bulk).
                if (CommonUtils.CompareTuples(_cmsisDapVersion, CMSISDAPVersion.V2_1_0) >= 0
                    || CommonUtils.CompareTuples(_cmsisDapVersion, CMSISDAPVersion.V1_3_0) >= 0 && CommonUtils.CompareTuples(_cmsisDapVersion, CMSISDAPVersion.V2_0_0) < 0)
                {
                    var fwVersionValue = Identify(ID.PRODUCT_FW_VERSION);
                    _fwVersion = fwVersionValue ?? "";
                }

                // Major protocol version based on use of bulk endpoints.
                var protoMajor = _interface.IsBulk ? 2 : 1;

                if (!string.IsNullOrEmpty(_fwVersion))
                {
                    Console.WriteLine($"CMSIS-DAP v{protoMajor} probe {_uniqueId ?? ""}: firmware version {_fwVersion}, protocol version {_cmsisDapVersion.Item1}.{_cmsisDapVersion.Item2}.{_cmsisDapVersion.Item3}");
                }
                else
                {
                    Console.WriteLine($"CMSIS-DAP v{protoMajor} probe {_uniqueId ?? ""}: protocol version {_cmsisDapVersion.Item1}.{_cmsisDapVersion.Item2}.{_cmsisDapVersion.Item3}");
                }

                _interface.SetPacketCount(_packetCount ?? 0);
                _packetSize = IdentityNumber(ID.MAX_PACKET_SIZE);
                _interface.PacketSize = _packetSize ?? 0;
                _capabilities = IdentityNumber(ID.CAPABILITIES);
                _hasSwoUart = (_capabilities & Capabilities.SWO_UART) != 0;
                if (_hasSwoUart)
                {
                    var swoBufferSizeValue = IdentityNumber(ID.SWO_BUFFER_SIZE);

                    if (swoBufferSizeValue > 0)
                    {
                        _swoBufferSize = swoBufferSizeValue;
                    }
                    else
                    {
                        _hasSwoUart = false;
                    }
                }
                else
                {
                    _swoBufferSize = 0;
                }

                _swoStatus = SWOStatus.DISABLED;

                InitDeferredBuffers();
                _hasOpenedOnce = true;
                _isOpen = true;
            }
        }

        public override void Close()
        {
            lock (_lock)
            {
                if (_interface == null)
                {
                    throw new ArgumentException();
                }

                if (!_isOpen)
                {
                    return;
                }

                Flush();
                _interface.Close();
                _isOpen = false;
                _crntCmd = new Command(0);
            }
        }

        public override string GetUniqueId()
        {
            return _uniqueId ?? "";
        }

        public byte PinAccess(byte mask, byte value)
        {
            lock (_lock)
            {
                Flush();
                return _protocol.SetSwjPins(value, mask);
            }
        }

        public override void AssertReset(bool asserted)
        {
            lock (_lock)
            {
                Flush();
                if (asserted)
                {
                    _protocol.SetSwjPins(0, Pin.nRESET);
                }
                else
                {
                    _protocol.SetSwjPins(Pin.nRESET, Pin.nRESET);
                }
            }
        }

        public override bool IsResetAsserted()
        {
            lock (_lock)
            {
                Flush();
                var pins = _protocol.SetSwjPins(0, Pin.NONE);
                return (pins & Pin.nRESET) == 0;
            }
        }

        public override void SetClock(int frequency)
        {
            lock (_lock)
            {
                Flush();
                _protocol.SetSwjClock(frequency);
                _frequency = frequency;
            }
        }

        public override PORT? GetSwjMode()
        {
            return _dapPort;
        }

        /// <summary>
        /// Allow transfers to be delayed and buffered
        /// </summary>
        /// <param name="enabled"></param>
        public override void SetDeferredTransfer(bool enabled)
        {
            /* By default deferred transfers are turned on.  When off, all reads and
            writes will be completed by the time the function returns.

            When enabled packets are buffered and sent all at once, which
            increases speed.  When memory is written to, the transfer
            might take place immediately, or might take place on a future
            memory write.  This means that an invalid write could cause an
            exception to occur on a later, unrelated write.  To guarantee
            that previous writes are complete call the flush() function. */

            if (_deferredTransfer && !enabled)
            {
                Flush();
            }

            _deferredTransfer = enabled;
        }

        public override void Flush()
        {
            lock (_lock)
            {
                // Send current packet
                SendPacket();

                // Read all backlogged
                for (int i = 0; i < _commandsToRead.Count; i++)
                {
                    ReadPacket();
                }
            }
        }

        public string? Identify(ID item)
        {
            lock (_lock)
            {
                // Check if this item has already been read and cached.
                if (_cachedInfo.ContainsKey(item))
                {
                    return _cachedInfo[item] as string;
                }

                // Check if buffers are inited before calling flush, so identify() can be called from open(), before the initing the deferred buffers.
                if (!_crntCmd.GetEmpty() || _commandsToRead.Count > 0)
                {
                    Flush();
                }

                var value = _protocol.DapInfo(item);
                _cachedInfo[item] = value;
                return value;
            }
        }

        public int IdentityNumber(ID item)
        {
            lock (_lock)
            {
                // Check if this item has already been read and cached.
                if (_cachedInfo.ContainsKey(item))
                {
                    var obj = _cachedInfo[item];
                    if (obj is int)
                    {
                        return (int)obj;
                    }
                    else
                    {
                        return 0;
                    }
                }

                // Check if buffers are inited before calling flush, so identify() can be called from open(), before the initing the deferred buffers.
                if (!_crntCmd.GetEmpty() || _commandsToRead.Count > 0)
                {
                    Flush();
                }

                var value = _protocol.DapInfo(item);
                _cachedInfo[item] = value;

                if (int.TryParse(value, out int result))
                {
                    return result;
                }
                else
                {
                    return 0;
                }
            }
        }

        public byte[] Vendor(byte index, byte[]? data = null)
        {
            lock (_lock)
            {
                data ??= [];

                Flush();

                return _protocol.Vendor(index, data);
            }
        }

        /* ------------------------------------------- *
         *             Target access functions
         * ------------------------------------------- */
        public void Connect(PORT port = PORT.DEFAULT)
        {
            lock (_lock)
            {
                var actualPort = _protocol.Connect((byte)port);
                _dapPort = (PORT)actualPort;
                // set clock frequency
                _protocol.SetSwjClock(_frequency);
                // configure transfer
                _protocol.TransferConfigure();

                // configure the selected protocol with defaults.
                if (_dapPort == PORT.SWD)
                {
                    ConfigureSwd();
                }
                else if (_dapPort == PORT.JTAG)
                {
                    ConfigureJtag();
                }

                // _protocol.SetLed(DAP_LED.DAP_DEBUGGER_CONNECTED, 1);
                // _protocol.SetLed(DAP_LED.DAP_TARGET_RUNNING, 0);
            }
        }



        public void ConfigureSwd(byte turnAround = 1, bool alwaysSendDataPhase = false)
        {
            lock (_lock)
            {
                Flush();
                _protocol.SwdConfigure(turnAround, alwaysSendDataPhase);
            }
        }

        public void ConfigureJtag(byte[]? devicesIrlen = null)
        {
            lock (_lock)
            {
                Flush();
                _protocol.JtagConfigure(devicesIrlen);
            }
        }

        public void SwjSequence(int length, long bits)
        {
            lock (_lock)
            {
                Flush();
                _protocol.SwjSequence(length, bits);
            }
        }

        public void SwdSequence(List<byte[]> sequences)
        {
            lock (_lock)
            {
                Flush();
                _protocol.SwdSequence(sequences);
            }
        }

        public void JtagSequence(byte cycles, byte tms, byte readTdo, int tdi)
        {
            lock (_lock)
            {
                Flush();
                _protocol.JtagSequence(cycles, tms, readTdo, tdi);
            }
        }

        public override void Disconnect()
        {
            lock (_lock)
            {
                Flush();
                // _protocol.SetLed(DAP_LED.DAP_DEBUGGER_CONNECTED, 0);
                // _protocol.SetLed(DAP_LED.DAP_TARGET_RUNNING, 0);
                _protocol.Disconnect();
            }
        }

        public override bool HasSwo
        {
            get
            {
                return _hasSwoUart;
            }
        }

        public override bool SwoConfigure(bool enabled, uint rate)
        {
            lock (_lock)
            {
                Flush();

                // Don't send any commands if the SWO commands aren't supported.
                if (!_hasSwoUart)
                {
                    return false;
                }

                // Before we attempt any configuration, we must explicitly disable SWO
                // (if SWO is enabled, setting any other configuration fails).
                SwoDisable();

                try
                {
                    if (enabled)
                    {
                        byte transport;
                        // Select the streaming SWO endpoint if available.
                        if (_interface.HasSwoEp)
                        {
                            transport = DAPSWOTransport.DAP_SWO_EP;
                        }
                        else
                        {
                            transport = DAPSWOTransport.DAP_SWO_DATA;
                        }

                        if (_protocol.SwoTransport(transport) != 0)
                        {
                            SwoDisable();
                            return false;
                        }

                        if (_protocol.SwoMode(DAPSWOMode.UART) != 0)
                        {
                            SwoDisable();
                            return false;
                        }

                        if (_protocol.SwoBaudrate(rate) != 0)
                        {
                            SwoDisable();
                            return false;
                        }

                        _swoStatus = SWOStatus.CONFIGURED;
                    }

                    return true;
                }
                catch (CommandError ex)
                {
                    Console.WriteLine($"Exception while configuring SWO: {ex.Message}");
                    SwoDisable();
                    return false;
                }
            }
        }

        // TODO PyUSBv2 implementation
        public override bool SwoControl(bool start)
        {
            lock (_lock)
            {
                Flush();

                // Don't send any commands if the SWO commands aren't supported.
                if (!_hasSwoUart)
                {
                    return false;
                }

                //if(start)
                //{
                //    _protocol.SwoControl(DAPSWOControl.START);
                //    if(_interface.HasSwoEp)
                //    {
                //        _interface.
                //    }
                //}

                return false;
            }
        }

        public (byte, uint) SwoStatus
        {
            get
            {
                lock (_lock)
                {
                    return _protocol.SwoStatus();
                }
            }
        }

        // TODO
        public byte[] SwoRead(int? count = null)
        {
            //if(_interface.HasSwoEp)
            //{

            //}

            if (count == null)
            {
                count = _packetSize;
            }

            // Must lock and flush the command queue since we're using the SWO read command that shares the command EP.

            return new byte[0];
        }

        public void WriteReg(REG regId, uint value, int dapIndex = 0)
        {
            byte request = WRITE;

            if ((int)regId < 4)
            {
                request |= DP_ACC;
            }
            else
            {
                request |= AP_ACC;
            }

            request |= (byte)((int)regId % 4 * 4);
            Write(dapIndex, 1, request, new uint[] { value });
        }

        public DelayResult<uint> ReadReg(REG regId, int dapIndex, bool now = true)
        {
            var request = READ;

            if ((int)regId < 4)
            {
                request |= DP_ACC;
            }
            else
            {
                request |= AP_ACC;
            }

            request |= (byte)((int)regId % 4 << 2);
            var transfer = Write(dapIndex, 1, request, null);

            if (transfer == null)
            {
                throw new Exception();
            }

            Func<uint> readRegCb = () =>
            {
                if (transfer == null)
                {
                    throw new ArgumentNullException();
                }

                var res = transfer.GetResult();
                if (res.Length != 1)
                {
                    throw new Exception($"res len is wrong expected 1, actual={res.Length}");
                }

                return res[0];
            };

            if (now)
            {
                return new DelayResult<uint>(readRegCb());
            }
            else
            {
                return new DelayResult<uint>(readRegCb);
            }
        }

        public void RegWriteRepeat(int numRepeats, REG regId, uint[] dataArray, int dapIndex = 0)
        {
            if (numRepeats != dataArray.Length)
            {
                throw new ArgumentException();
            }

            var request = WRITE;

            if ((int)regId < 4)
            {
                request |= DP_ACC;
            }
            else
            {
                request |= AP_ACC;
            }

            request |= (byte)((int)regId % 4 * 4);
            Write(dapIndex, numRepeats, request, dataArray);
        }

        public DelayResult<uint[]> RegReadRepeat(int numRepeats, REG regId, int dapIndex = 0, bool now = true)
        {
            var request = READ;

            if ((int)regId < 4)
            {
                request |= DP_ACC;
            }
            else
            {
                request |= AP_ACC;
            }

            request |= (byte)((int)regId % 4 * 4);
            var transfer = Write(dapIndex, numRepeats, request, null);
            if (transfer == null)
            {
                throw new Exception();
            }

            Func<uint[]> action = () =>
            {
                if (transfer == null)
                {
                    throw new ArgumentNullException();
                }

                var res = transfer.GetResult();
                if (res.Length != numRepeats)
                {
                    throw new Exception();
                }

                return res;
            };

            if (now)
            {
                return new DelayResult<uint[]>(action());
            }
            else
            {
                return new DelayResult<uint[]>(action);
            }
        }

        public DelayResult<uint[]> ReadApMultiple(uint addr, int count, bool now = true)
        {
            var apReg = REG_ADDR_TO_ID_MAP[(AP, addr & A32)];

            DelayResult<uint[]>? result = null;

            try
            {
                result = RegReadRepeat(count, apReg, 0, now);
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.ToString());
            }

            // Need to wrap the deferred callback to convert exceptions.
            var readApRepeatCallback = () =>
            {
                try
                {
                    if (result != null && result.Action != null)
                    {
                        var values = result.Action();
                        return values;
                    }
                    else
                    {
                        return [];
                    }
                }
                catch (Exception ex)
                {
                    Console.WriteLine(ex.ToString());
                    return [];
                }
            };

            if (now)
            {
                return result;
            }
            else
            {
                return new DelayResult<uint[]>(readApRepeatCallback);
            }
        }

        public void WriteApMultiple(uint addr, uint[] values)
        {
            var apReg = REG_ADDR_TO_ID_MAP[(AP, addr & A32)];

            try
            {
                RegWriteRepeat(values.Length, apReg, values); ;
                // Console.WriteLine($"trace: write_ap_multi(addr=%{addr:X8})");
            }
            catch (Exception ex)
            {
                Console.WriteLine($"trace: write_ap_multi(addr=%{addr:X8}), error:{ex.Message}");
            }
        }

        /// <summary>
        /// Initialize or reinitialize all the deferred transfer buffers, Calling this method will drop all pending transactions, so use with care.
        /// </summary>
        private void InitDeferredBuffers()
        {
            // List of transfers that have been started, but
            // not completed (started by write_reg, read_reg,
            // reg_write_repeat and reg_read_repeat)
            _transferList.Clear();
            // The current packet - this can contain multiple
            // different transfers
            _crntCmd = new Command(_packetSize ?? 0);
            // Packets that have been sent but not read
            _commandsToRead.Clear();
            // Buffer for data returned for completed commands.
            // This data will be added to transfers
            _commandResponseBuf = new byte[0];
        }

        /// <summary>
        /// Doesn't need @locked because it is only called from swo_configure().
        /// </summary>
        private void SwoDisable()
        {
            try
            {
                _protocol.SwoMode(DAPSWOMode.OFF);
                _protocol.SwoTransport(DAPSWOTransport.NONE);
            }
            catch (Exception ex)
            {
                Console.WriteLine($"Exception while disabling SWO: {ex.Message}");
            }
            finally
            {
                _swoStatus = SWOStatus.DISABLED;
            }
        }

        private void PrintData(byte[] data)
        {
            Console.Write("send packet:");
            foreach (var item in data)
            {
                Console.Write(item.ToString("x2"));
            }
            Console.WriteLine();
        }

        /// <summary>
        /// Send a single packet to the interface
        /// This function guarantees that the number of packets
        /// that are stored in daplink's buffer (the number of
        /// packets written but not read) does not exceed the
        /// number supported by the given device.
        /// </summary>
        private void SendPacket()
        {
            lock (_lock)
            {
                var cmd = _crntCmd;
                if (cmd.GetEmpty())
                {
                    return;
                }

                var maxPackets = _interface.PacketCount;
                if (_commandsToRead.Count >= maxPackets)
                {
                    // Console.WriteLine($"[cmd:{cmd.Uid}] _send_packet: reading packet; outstanding={_commandsToRead.Count} >= max={maxPackets}");
                    ReadPacket();
                }
                // Console.WriteLine($"[cmd:{cmd.Uid}] _send_packet: sending, {DateTime.Now:yyyy/MM/dd HH:mm:ss.fff}");
                var data = cmd.EncodeData();

                try
                {
                    // PrintData(data);
                    _interface.Write(data);
                }
                catch (Exception ex)
                {
                    AbortAllTransfers(ex);
                    throw;
                }

                _commandsToRead.AddLast(cmd);
                _crntCmd = new Command(_packetSize ?? 0);
            }
        }

        /// <summary>
        /// Reads and decodes a single packet
        /// Reads a single packet from the device and
        /// stores the data from it in the current Command
        /// object
        /// </summary>
        internal void ReadPacket()
        {
            // Grab command, send it and decode response
            lock (_lock)
            {
                var cmd = _commandsToRead.First();
                _commandsToRead.RemoveFirst();

                // Console.WriteLine($"[cmd:{cmd.Uid}] _read_packet: reading, {DateTime.Now:yyyy/MM/dd HH:mm:ss.fff}");

                byte[] decodedData;

                try
                {
                    var rawData = _interface.Read();
                    //foreach (var packet in rawData)
                    //{
                    //    Console.Write($"{packet:x2}");
                    //}
                    //Console.WriteLine();
                    decodedData = cmd.DecodeData(rawData);
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"[cmd:{cmd.Uid}] _read_packet: got exception {ex.Message}; aborting all transfers!");
                    AbortAllTransfers(ex);
                    throw;
                }

                _commandResponseBuf = _commandResponseBuf.Concat(decodedData).ToArray();

                // Attach data to transfers
                var pos = 0;
                while (true)
                {
                    var sizeLeft = _commandResponseBuf.Length - pos;
                    if (sizeLeft == 0)
                    {
                        // If size left is 0 then the transfer list might
                        // be empty, so don't try to access element 0
                        break;
                    }

                    var transfer = _transferList.First();
                    var size = transfer.DataSize;
                    if (size > sizeLeft)
                    {
                        break;
                    }

                    _transferList.RemoveFirst();
                    var data = _commandResponseBuf.Skip(pos).Take(size).ToArray();
                    pos += size;
                    transfer.AddResponse(data);
                }

                // Remove used data from _command_response_buf
                if (pos > 0)
                {
                    _commandResponseBuf = _commandResponseBuf.Skip(pos).ToArray();
                }
            }
        }

        /// <summary>
        /// Abort any ongoing transfers and clear all buffers
        /// </summary>
        /// <param name="ex"></param>
        private void AbortAllTransfers(Exception ex)
        {
            lock (_lock)
            {
                var pendingReads = _commandsToRead.Count;
                Console.WriteLine($"aborting {pendingReads} pending reads after exception {ex.Message}");

                // invalidate _transfer_list
                foreach (var transfer in _transferList)
                {
                    transfer.AddError(ex);
                }

                // clear all deferred buffers
                InitDeferredBuffers();

                // finish all pending reads and ignore the data
                // Only do this if the error is a transfer error.
                // Otherwise this could cause another exception
                if (ex is TransferError)
                {
                    for (var i = 0; i < pendingReads; i++)
                    {
                        _interface.Read();
                    }
                }
            }
        }

        public void WriteDP(uint addr, uint data)
        {
            var regId = REG_ADDR_TO_ID_MAP[(DP, addr)];

            // # Write the DP register.
            try
            {
                WriteReg(regId, data);
            }
            catch (Exception)
            {
                throw;
            }
        }

        public void WriteAP(uint addr, uint data)
        {
            var apReg = REG_ADDR_TO_ID_MAP[(AP, addr & A32)];
            try
            {
                // # Perform the AP register write.
                WriteReg(apReg, data);
            }
            catch (Exception)
            {
                throw;
            }
        }

        public DelayResult<uint> ReadAP(uint addr, bool now = true)
        {
            var apReg = REG_ADDR_TO_ID_MAP[(AP, addr & A32)];

            DelayResult<uint> resultCb;
            try
            {
                if (!now)
                {
                    // TODO
                    // Console.WriteLine($"TODO trace: read_ap(addr=#{addr:X8}) -> ...");

                }
                resultCb = ReadReg(apReg, 0, now);
            }
            catch (Exception)
            {
                throw;
            }

            // Read callback returned for async reads.
            Func<uint> readDPCb = () =>
            {
                try
                {
                    uint result;

                    if (resultCb.IsValue)
                    {
                        result = resultCb.Value;
                    }
                    else
                    {
                        if (resultCb.Action != null)
                        {
                            result = resultCb.Action();
                        }
                        else
                        {
                            throw new Exception();
                        }
                    }

                    return result;
                }
                catch (Exception)
                {
                    throw;
                }
            };

            if (now)
            {
                return new DelayResult<uint>(readDPCb());
            }
            else
            {
                return new DelayResult<uint>(readDPCb);
            }
        }

        public DelayResult<uint> ReadDP(uint addr, bool now = true)
        {
            var regId = REG_ADDR_TO_ID_MAP[(DP, addr)];

            DelayResult<uint> resultCb;
            try
            {
                if (!now)
                {
                    // Console.WriteLine($"TODO trace: read_dp(addr=#{addr:X8}) -> ...");
                }

                resultCb = ReadReg(regId, dapIndex: 0, now);
            }
            catch (Exception ex)
            {
                Console.WriteLine($"trace: read_dp(addr=#{addr:X8}) -> error({ex.Message})");
                throw;
            }

            // Read callback returned for async reads.
            Func<uint> readDPCb = () =>
            {
                try
                {
                    uint result;

                    if (resultCb.IsValue)
                    {
                        result = resultCb.Value;
                    }
                    else
                    {
                        if (resultCb.Action != null)
                        {
                            result = resultCb.Action();
                        }
                        else
                        {
                            throw new Exception();
                        }
                    }

                    return result;
                }
                catch (Exception)
                {
                    throw;
                }
            };

            if (now)
            {
                return new DelayResult<uint>(readDPCb());
            }
            else
            {
                return new DelayResult<uint>(readDPCb);
            }
        }

        private Transfer? Write(int dapIndex, int transferCount, byte transferRequest, uint[]? transferData)
        {
            lock (_lock)
            {
                Transfer? transfer = null;
                // Create transfer and add to transfer list
                if ((transferRequest & READ) != 0)
                {
                    transfer = new Transfer(this, dapIndex, transferCount, transferRequest);
                    _transferList.AddLast(transfer);
                }

                // Build physical packet by adding it to command
                var cmd = _crntCmd;
                var sizeToTransfer = transferCount;
                var transDataPos = 0;

                while (sizeToTransfer > 0)
                {
                    // Get the size remaining in the current packet for the given request.
                    var size = cmd.GetRequestSpace(sizeToTransfer, transferRequest, dapIndex);

                    // This request doesn't fit in the packet so send it.
                    if (size == 0)
                    {
                        Console.WriteLine("_write: send packet [size==0]");
                        SendPacket();
                        cmd = _crntCmd;
                        continue;
                    }

                    uint[]? data = null;
                    // Add request to packet.
                    if (transferData == null)
                    {
                        data = null;
                    }
                    else
                    {
                        data = transferData.Skip(transDataPos).Take(size).ToArray();
                    }

                    cmd.Add(size, transferRequest, data, dapIndex);
                    sizeToTransfer -= size;
                    transDataPos += size;

                    // Packet has been filled so send it
                    if (cmd.GetFull())
                    {
                        // Console.WriteLine("_write: send packet [full]");
                        SendPacket();
                        cmd = _crntCmd;
                    }
                }

                if (!_deferredTransfer)
                {
                    Flush();
                }

                return transfer;
            }
        }

        internal class Command
        {
            internal const int _UNSET_DAP_INDEX = -1;

            private int _commandCounter = 0;
            private int _id;
            private int _size;
            private int _readCount = 0;
            private int _writeCount = 0;
            private bool _blockAllowed = true;
            private uint? _blockRequest = null;
            private List<(int, uint, uint[])> _data = [];
            private int _dapIndex = _UNSET_DAP_INDEX;
            private bool _dataEncoded = false;

            public Command(int size)
            {
                _id = _commandCounter;
                _commandCounter++;
                _size = size;
            }

            public int Uid
            {
                get { return _id; }
            }

            private int GetFreeTransfers(bool blockAllowed, bool isRead)
            {
                int send = 0;
                int recv = 0;

                // Return the number of available read or write transfers.
                if (blockAllowed)
                {
                    // DAP_TransferBlock request packet:
                    // BYTE | BYTE *****| SHORT**********| BYTE *************| WORD *********|
                    // > 0x06 | DAP Index | Transfer Count | Transfer Request  | Transfer Data |
                    //  ******|***********|****************|*******************|+++++++++++++++|
                    send = _size - 5 - 4 * _writeCount;

                    // DAP_TransferBlock response packet:
                    // BYTE | SHORT *********| BYTE *************| WORD *********|
                    // < 0x06 | Transfer Count | Transfer Response | Transfer Data |
                    //  ******|****************|*******************|+++++++++++++++|
                    recv = _size - 4 - 4 * _readCount;

                    if (isRead)
                    {
                        return recv / 4;
                    }
                    else
                    {
                        return send / 4;
                    }
                }
                else
                {
                    // DAP_Transfer request packet:
                    // BYTE | BYTE *****| BYTE **********| BYTE *************| WORD *********|
                    // > 0x05 | DAP Index | Transfer Count | Transfer Request  | Transfer Data |
                    //  ******|***********|****************|+++++++++++++++++++++++++++++++++++|
                    send = _size - 3 - 1 * _readCount - 5 * _writeCount;

                    // DAP_Transfer response packet:
                    // BYTE | BYTE **********| BYTE *************| WORD *********|
                    // < 0x05 | Transfer Count | Transfer Response | Transfer Data |
                    //  ******|****************|*******************|+++++++++++++++|
                    recv = _size - 3 - 4 * _readCount;

                    if (isRead)
                    {
                        // 1 request byte in request packet, 4 data bytes in response packet
                        return Math.Min(send, recv / 4);
                    }
                    else
                    {
                        // 1 request byte + 4 data bytes
                        return send / 5;
                    }
                }
            }

            public int GetRequestSpace(int count, uint request, int dapIndex)
            {
                if (_dataEncoded)
                {
                    throw new InvalidOperationException("_dataEncoded应为false");
                }

                // Must create another command if the dap index is different.
                if (_dapIndex != _UNSET_DAP_INDEX && dapIndex != _dapIndex)
                {
                    return 0;
                }

                // Block transfers must use the same request.
                var blockAllowed = _blockAllowed;
                if (_blockRequest != null && request != _blockRequest.Value)
                {
                    blockAllowed = false;
                }

                // Compute the portion of the request that will fit in this packet.
                var isRead = (request & READ) != 0;
                var free = GetFreeTransfers(blockAllowed, isRead);
                var size = Math.Min(count, free);

                // Non-block transfers only have 1 byte for request count.
                if (!blockAllowed)
                {
                    var maxCount = _writeCount + _readCount + size;
                    var delta = maxCount - 255;
                    size = Math.Min(size - delta, size);
                }
                else
                {
                    // trace
                }

                // We can get a negative free count if the packet already contains more data than can be
                // sent by a DAP_Transfer command, but the new request forces DAP_Transfer. In this case,
                // just return 0 to force the DAP_Transfer_Block to be sent.
                return Math.Max(size, 0);
            }

            public bool GetFull()
            {
                return GetFreeTransfers(_blockAllowed, true) == 0 || GetFreeTransfers(_blockAllowed, false) == 0;
            }

            public bool GetEmpty()
            {
                // Return True if no transfers have been added to this packet
                return _data.Count == 0;
            }

            // Add a single or block register transfer operation to this command
            public void Add(int count, uint request, uint[] data, int dapIndex)
            {
                if (_dataEncoded)
                {
                    throw new InvalidOperationException("_dataEncoded应为false");
                }

                if (_dapIndex == _UNSET_DAP_INDEX)
                {
                    _dapIndex = dapIndex;
                }

                if (_dapIndex != dapIndex)
                {
                    throw new InvalidOperationException("_dapIndex应与dapIndex一致");
                }

                if (_blockRequest == null)
                {
                    _blockRequest = request;
                }
                else if (request != _blockRequest)
                {
                    _blockAllowed = false;
                }

                if (!(!_blockAllowed || _blockRequest == request))
                {
                    throw new InvalidOperationException();
                }

                if ((request & READ) != 0)
                {
                    _readCount += count;
                }
                else
                {
                    _writeCount += count;
                }

                _data.Add((count, request, data));
            }

            /// <summary>
            /// Encode this command into a byte array that can be sent
            /// </summary>
            /// <returns>The data returned by this function is a bytearray in
            /// the format that of a DAP_Transfer CMSIS-DAP command.</returns>
            private byte[] EncodeTransferData()
            {
                if (GetEmpty())
                {
                    throw new InvalidOperationException();
                }

                var buf = new byte[_size];
                var transferCount = _readCount + _writeCount;
                var pos = 0;
                buf[pos++] = CommandConsts.DAP_TRANSFER;
                buf[pos++] = (byte)_dapIndex;
                buf[pos++] = (byte)transferCount;

                foreach (var (count, request, writeList) in _data)
                {
                    if (!(writeList == null || writeList.Length <= count))
                    {
                        throw new InvalidOperationException();
                    }

                    var writePos = 0;
                    for (var i = 0; i < count; i++)
                    {
                        buf[pos++] = (byte)request;

                        if ((request & READ) == 0)
                        {
                            if (writeList == null)
                            {
                                throw new InvalidOperationException();
                            }

                            buf[pos++] = (byte)(writeList[writePos] >> 8 * 0 & 0xff);
                            buf[pos++] = (byte)(writeList[writePos] >> 8 * 1 & 0xff);
                            buf[pos++] = (byte)(writeList[writePos] >> 8 * 2 & 0xff);
                            buf[pos++] = (byte)(writeList[writePos] >> 8 * 3 & 0xff);

                            writePos++;
                        }
                    }
                }

                return buf.Take(pos).ToArray();
            }

            /// <summary>
            /// Check the response status byte from CMSIS-DAP transfer commands.
            /// The ACK bits [2:0] and the protocol error bit are checked. If any error is indicated,
            /// the appropriate exception is raised.An exception is also raised for unrecognised ACK
            /// values.
            /// </summary>
            /// <param name="response">he "Transfer Response" byte from a DAP_Transfer or DAP_TransferBlock command.</param>
            /// <exception cref="DAPAccessIntf.TransferFaultError">Raised for the ACK_FAULT response.</exception>
            /// <exception cref="DAPAccessIntf.TransferTimeoutError">Raised for ACK_WAIT response.</exception>
            /// <exception cref="DAPAccessIntf.TransferError">Raised for other, less common errors, including No
            /// ACK, SWD protocol error, and an unknown ACK value.A descriptive error message is used
            /// to indicate which of these errors was the cause.</exception>
            private void CheckResponse(byte response)
            {
                var ack = response & DAPTransferResponse.ACK_MASK;
                if (ack != DAPTransferResponse.ACK_OK)
                {
                    if (ack == DAPTransferResponse.ACK_FAULT)
                    {
                        throw new TransferFaultError();
                    }
                    else if (ack == DAPTransferResponse.ACK_WAIT)
                    {
                        throw new TransferTimeoutError();
                    }
                    else if (ack == DAPTransferResponse.ACK_NO_ACK)
                    {
                        throw new TransferError("No ACK received");
                    }
                    else
                    {
                        throw new TransferError($"Unexpected ACK value {ack} returned by probe");
                    }
                }
                else if ((response & DAPTransferResponse.PROTOCOL_ERROR_MASK) != 0)
                {
                    throw new TransferError("SWD protocol error");
                }
            }

            /// <summary>
            /// Take a byte array and extract the data from it
            /// Decode the response returned by a DAP_Transfer CMSIS-DAP command
            /// and return it as an array of bytes.
            /// </summary>
            /// <param name="data"></param>
            /// <returns></returns>
            private byte[] DecodeTransferData(byte[] data)
            {
                if (GetEmpty())
                {
                    throw new InvalidOperationException();
                }

                if (data[0] != CommandConsts.DAP_TRANSFER)
                {
                    throw new TransferError($"DAP_TRANSFER response error: response is for command {data[0]:x2}");
                }

                // Check response and raise an exception on errors.
                CheckResponse(data[2]);

                // Check for count mismatch after checking for DAP_TRANSFER_FAULT
                // This allows TransferFaultError or TransferTimeoutError to get
                // thrown instead of TransferFaultError
                if (data[1] != _readCount + _writeCount)
                {
                    throw new TransferError();
                }

                return data.Skip(3).Take(4 * _readCount).ToArray();
            }

            /// <summary>
            /// Encode this command into a byte array that can be sent
            /// </summary>
            /// <returns>The data returned by this function is a bytearray in the format that of a DAP_TransferBlock CMSIS-DAP command.</returns>
            private byte[] EncodeTransferBlockData()
            {
                if (GetEmpty())
                {
                    throw new InvalidOperationException();
                }

                var buf = new byte[_size];

                var transferCount = _readCount + _writeCount;
                if (_readCount != 0 && _writeCount != 0)
                {
                    throw new InvalidOperationException();
                }

                if (_blockRequest == null)
                {
                    throw new InvalidOperationException();
                }

                var pos = 0;

                buf[pos++] = CommandConsts.DAP_TRANSFER_BLOCK;
                buf[pos++] = (byte)_dapIndex;
                buf[pos++] = (byte)(transferCount & 0xff);
                buf[pos++] = (byte)(transferCount >> 8 & 0xff);
                buf[pos++] = (byte)_blockRequest.Value;

                foreach (var (count, request, writeList) in _data)
                {
                    if (!(writeList == null || writeList.Length <= count))
                    {
                        throw new InvalidOperationException();
                    }

                    if (request != _blockRequest)
                    {
                        throw new InvalidOperationException();
                    }

                    var writePos = 0;
                    if ((request & READ) == 0)
                    {
                        if (writeList == null)
                        {
                            throw new NullReferenceException();
                        }

                        for (var i = 0; i < count; i++)
                        {
                            buf[pos++] = (byte)(writeList[writePos] >> 8 * 0 & 0xff);
                            buf[pos++] = (byte)(writeList[writePos] >> 8 * 1 & 0xff);
                            buf[pos++] = (byte)(writeList[writePos] >> 8 * 2 & 0xff);
                            buf[pos++] = (byte)(writeList[writePos] >> 8 * 3 & 0xff);

                            writePos++;
                        }
                    }
                }

                return buf.Take(pos).ToArray();
            }

            /// <summary>
            /// Take a byte array and extract the data from it
            /// </summary>
            /// <param name="data"></param>
            /// <returns>Decode the response returned by a DAP_TransferBlock CMSIS-DAP command and return it as an array of bytes.</returns>
            private byte[] DecodeTransferBlockData(byte[] data)
            {
                if (GetEmpty())
                {
                    throw new InvalidOperationException();
                }

                if (data[0] != CommandConsts.DAP_TRANSFER_BLOCK)
                {
                    throw new TransferError($"DAP_TRANSFER_BLOCK response error: response is for command {data[0]:x2}");
                }

                // Check response and raise an exception on errors.
                CheckResponse(data[3]);

                // Check for count mismatch after checking for DAP_TRANSFER_FAULT
                // This allows TransferFaultError or TransferTimeoutError to get
                // thrown instead of TransferFaultError
                var transferCount = data[1] | data[2] << 8;
                if (transferCount != _readCount + _writeCount)
                {
                    throw new TransferError();
                }

                return data.Skip(4).Take(4 * _readCount).ToArray();
            }

            /// <summary>
            /// Encode this command into a byte array that can be sent
            /// </summary>
            /// <returns>The actual command this is encoded into depends on the data that was added.</returns>
            public byte[] EncodeData()
            {
                if (GetEmpty())
                {
                    throw new InvalidOperationException();
                }

                byte[] data;

                _dataEncoded = true;
                if (_blockAllowed)
                {
                    data = EncodeTransferBlockData();
                }
                else
                {
                    data = EncodeTransferData();
                }

                return data;
            }

            /// <summary>
            /// Decode the response data
            /// </summary>
            /// <param name="data"></param>
            /// <returns></returns>
            public byte[] DecodeData(byte[] data)
            {
                if (GetEmpty())
                {
                    throw new InvalidOperationException();
                }

                if (_dataEncoded == false)
                {
                    throw new InvalidOperationException();
                }

                byte[] dataDecoded;
                if (_blockAllowed)
                {
                    dataDecoded = DecodeTransferBlockData(data);
                }
                else
                {
                    dataDecoded = DecodeTransferData(data);
                }

                return dataDecoded;
            }


        }

        /// <summary>
        /// A wrapper object representing a command invoked by the layer above.
        /// The transfer class contains a logical register read or a block
        /// of reads to the same register.
        /// </summary>

    }

    public class Transfer
    {
        private readonly DapAccessCmsisDap _daplink;
        private int _dapIndex;
        private int _transferCount;
        private byte _transferRequest;
        private int _sizeBytes = 0;
        private uint[]? _result = null;
        private Exception? _error;

        public Transfer(DapAccessCmsisDap daplink, int dapIndex, int transferCount, byte transferRequest)
        {
            _daplink = daplink;
            _dapIndex = dapIndex;
            _transferCount = transferCount;
            _transferRequest = transferRequest;
            _result = null;

            if ((transferRequest & DapAccessCmsisDap.READ) != 0)
            {
                _sizeBytes = transferCount * 4;
            }
        }

        public int DataSize => _sizeBytes;

        /// <summary>
        /// Add data read from the remote device to this object.
        /// The size of data added must match exactly the size
        /// that get_data_size returns.
        /// </summary>
        public void AddResponse(byte[] data)
        {
            if (data.Length != _sizeBytes)
            {
                throw new ArgumentException();
            }

            var result = new List<uint>();
            for (int i = 0; i < _sizeBytes / 4; i++)
            {
                result.Add((uint)(data[0 + i * 4] << 0 | data[1 + i * 4] << 8 | data[2 + i * 4] << 16 | data[3 + i * 4] << 24));
            }
            _result = result.ToArray();
        }

        /// <summary>
        /// Attach an exception to this transfer rather than data.
        /// </summary>
        /// <param name="ex"></param>
        public void AddError(Exception ex)
        {
            if (ex is not Exception)
            {
                throw new ArgumentException();
            }

            _error = ex;
        }

        /// <summary>
        /// Get the result of this transfer.
        /// </summary>
        /// <returns></returns>
        public uint[] GetResult()
        {
            while (_result == null)
            {
                if (_daplink.CommandsToRead.Count > 0)
                {
                    _daplink.ReadPacket();
                }
                else
                {
                    if (_daplink.CrntCmd.GetEmpty())
                    {
                        throw new ArgumentException();
                    }
                    _daplink.Flush();
                }
            }

            if (_error != null)
            {
                throw _error;
            }

            if (_result == null)
            {
                throw new ArgumentException();
            }

            return _result;
        }
    }
}
